import os
import yaml

import ament_index_python.packages
from launch import LaunchDescription
from launch_ros.actions import ComposableNodeContainer
from launch_ros.descriptions import ComposableNode


def generate_launch_description():
    driver_share_dir = ament_index_python.packages.get_package_share_directory('lidar209_driver')
    driver_params_file = os.path.join(driver_share_dir, 'config', 'driver_node_params.yaml')
    with open(driver_params_file, 'r') as f:
        driver_params = yaml.safe_load(f)['lidar209_driver_node']['ros__parameters']

    convert_share_dir = ament_index_python.packages.get_package_share_directory('to_pointcloud')
    convert_params_file = os.path.join(convert_share_dir, 'config', 'qs176_cloud_params.yaml')
    with open(convert_params_file, 'r') as f:
        convert_params = yaml.safe_load(f)['to_pointcloud_node']['ros__parameters']
    convert_params['calibration'] = os.path.join(convert_share_dir, 'params', 'CZLidar_QS176_ZW1_34.yaml')

    container = ComposableNodeContainer(
            name='lidar209_container',
            namespace='',
            package='rclcpp_components',
            executable='component_container',
            composable_node_descriptions=[
                ComposableNode(
                    package='lidar209_driver',
                    plugin='lidar209_driver::lidar209Driver',
                    name='lidar209_driver_node',
                    parameters=[driver_params]),
                ComposableNode(
                    package='to_pointcloud',
                    plugin='lidar209_pointcloud::Convert',
                    name='to_pointcloud_node',
                    parameters=[convert_params]),
            ],
            output='both',
    )

    return LaunchDescription([container])
